diff --git a/board/freescale/common/Makefile b/board/freescale/common/Makefile
index 22b57cc..50d7731 100644
--- a/board/freescale/common/Makefile
+++ b/board/freescale/common/Makefile
@@ -34,6 +34,8 @@
 obj-$(CONFIG_FSL_FIXED_MMC_LOCATION)	+= sdhc_boot.o
 endif
 
+obj-$(CONFIG_FSL_DIU_CH7301)	+= diu_ch7301.o
+
 obj-$(CONFIG_MPC8541CDS)	+= cds_pci_ft.o
 obj-$(CONFIG_MPC8548CDS)	+= cds_pci_ft.o
 obj-$(CONFIG_MPC8555CDS)	+= cds_pci_ft.o
diff --git a/board/freescale/common/diu_ch7301.c b/board/freescale/common/diu_ch7301.c
new file mode 100644
index 0000000..82ce870
--- /dev/null
+++ b/board/freescale/common/diu_ch7301.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright 2014 Freescale Semiconductor, Inc.
+ * Authors: Priyanka Jain <Priyanka.Jain@freescale.com>
+ *	    Wang Dongsheng <dongsheng.wang@freescale.com>
+ *
+ * This file is copied and modified from the original t1040qds/diu.c.
+ * Encoder can be used in T104x and LSx Platform.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <stdio_dev.h>
+#include <i2c.h>
+
+#define I2C_DVI_INPUT_DATA_FORMAT_REG		0x1F
+#define I2C_DVI_PLL_CHARGE_CNTL_REG		0x33
+#define I2C_DVI_PLL_DIVIDER_REG			0x34
+#define I2C_DVI_PLL_SUPPLY_CNTL_REG		0x35
+#define I2C_DVI_PLL_FILTER_REG			0x36
+#define I2C_DVI_TEST_PATTERN_REG		0x48
+#define I2C_DVI_POWER_MGMT_REG			0x49
+#define I2C_DVI_LOCK_STATE_REG			0x4D
+#define I2C_DVI_SYNC_POLARITY_REG		0x56
+
+/*
+ * Set VSYNC/HSYNC to active high. This is polarity of sync signals
+ * from DIU->DVI. The DIU default is active igh, so DVI is set to
+ * active high.
+ */
+#define I2C_DVI_INPUT_DATA_FORMAT_VAL		0x98
+
+#define I2C_DVI_PLL_CHARGE_CNTL_HIGH_SPEED_VAL	0x06
+#define I2C_DVI_PLL_DIVIDER_HIGH_SPEED_VAL	0x26
+#define I2C_DVI_PLL_FILTER_HIGH_SPEED_VAL	0xA0
+#define I2C_DVI_PLL_CHARGE_CNTL_LOW_SPEED_VAL	0x08
+#define I2C_DVI_PLL_DIVIDER_LOW_SPEED_VAL	0x16
+#define I2C_DVI_PLL_FILTER_LOW_SPEED_VAL	0x60
+
+/* Clear test pattern */
+#define I2C_DVI_TEST_PATTERN_VAL		0x18
+/* Exit Power-down mode */
+#define I2C_DVI_POWER_MGMT_VAL			0xC0
+
+/* Monitor polarity is handled via DVI Sync Polarity Register */
+#define I2C_DVI_SYNC_POLARITY_VAL		0x00
+
+/* Programming of HDMI Chrontel CH7301 connector */
+int diu_set_dvi_encoder(unsigned int pixclock)
+{
+	int ret;
+	u8 temp;
+
+	temp = I2C_DVI_TEST_PATTERN_VAL;
+	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_TEST_PATTERN_REG, 1,
+			&temp, 1);
+	if (ret) {
+		puts("I2C: failed to select proper dvi test pattern\n");
+		return ret;
+	}
+	temp = I2C_DVI_INPUT_DATA_FORMAT_VAL;
+	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_INPUT_DATA_FORMAT_REG,
+			1, &temp, 1);
+	if (ret) {
+		puts("I2C: failed to select dvi input data format\n");
+		return ret;
+	}
+
+	/* Set Sync polarity register */
+	temp = I2C_DVI_SYNC_POLARITY_VAL;
+	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_SYNC_POLARITY_REG, 1,
+			&temp, 1);
+	if (ret) {
+		puts("I2C: failed to select dvi syc polarity\n");
+		return ret;
+	}
+
+	/* Set PLL registers based on pixel clock rate*/
+	if (pixclock > 65000000) {
+		temp = I2C_DVI_PLL_CHARGE_CNTL_HIGH_SPEED_VAL;
+		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
+				I2C_DVI_PLL_CHARGE_CNTL_REG, 1,	&temp, 1);
+		if (ret) {
+			puts("I2C: failed to select dvi pll charge_cntl\n");
+			return ret;
+		}
+		temp = I2C_DVI_PLL_DIVIDER_HIGH_SPEED_VAL;
+		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
+				I2C_DVI_PLL_DIVIDER_REG, 1, &temp, 1);
+		if (ret) {
+			puts("I2C: failed to select dvi pll divider\n");
+			return ret;
+		}
+		temp = I2C_DVI_PLL_FILTER_HIGH_SPEED_VAL;
+		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
+				I2C_DVI_PLL_FILTER_REG, 1, &temp, 1);
+		if (ret) {
+			puts("I2C: failed to select dvi pll filter\n");
+			return ret;
+		}
+	} else {
+		temp = I2C_DVI_PLL_CHARGE_CNTL_LOW_SPEED_VAL;
+		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
+				I2C_DVI_PLL_CHARGE_CNTL_REG, 1, &temp, 1);
+		if (ret) {
+			puts("I2C: failed to select dvi pll charge_cntl\n");
+			return ret;
+		}
+		temp = I2C_DVI_PLL_DIVIDER_LOW_SPEED_VAL;
+		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
+				I2C_DVI_PLL_DIVIDER_REG, 1, &temp, 1);
+		if (ret) {
+			puts("I2C: failed to select dvi pll divider\n");
+			return ret;
+		}
+		temp = I2C_DVI_PLL_FILTER_LOW_SPEED_VAL;
+		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
+				I2C_DVI_PLL_FILTER_REG, 1, &temp, 1);
+		if (ret) {
+			puts("I2C: failed to select dvi pll filter\n");
+			return ret;
+		}
+	}
+
+	temp = I2C_DVI_POWER_MGMT_VAL;
+	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_POWER_MGMT_REG, 1,
+			&temp, 1);
+	if (ret) {
+		puts("I2C: failed to select dvi power mgmt\n");
+		return ret;
+	}
+
+	udelay(500);
+
+	return 0;
+}
diff --git a/board/freescale/common/diu_ch7301.h b/board/freescale/common/diu_ch7301.h
new file mode 100644
index 0000000..8b6ead0
--- /dev/null
+++ b/board/freescale/common/diu_ch7301.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright 2014 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#ifndef __DIU_HDMI_CH7301__
+#define __DIU_HDMI_CH7301__
+
+/* Programming of HDMI Chrontel CH7301 connector */
+int diu_set_dvi_encoder(unsigned int pixclock);
+
+#endif
diff --git a/board/freescale/t1040qds/diu.c b/board/freescale/t1040qds/diu.c
index ffd074b..0214224 100644
--- a/board/freescale/t1040qds/diu.c
+++ b/board/freescale/t1040qds/diu.c
@@ -13,42 +13,9 @@
 #include <video_fb.h>
 #include <fsl_diu_fb.h>
 #include "../common/qixis.h"
+#include "../common/diu_ch7301.h"
 #include "t1040qds.h"
 #include "t1040qds_qixis.h"
-#include <i2c.h>
-
-
-#define I2C_DVI_INPUT_DATA_FORMAT_REG		0x1F
-#define I2C_DVI_PLL_CHARGE_CNTL_REG		0x33
-#define I2C_DVI_PLL_DIVIDER_REG			0x34
-#define I2C_DVI_PLL_SUPPLY_CNTL_REG		0x35
-#define I2C_DVI_PLL_FILTER_REG			0x36
-#define I2C_DVI_TEST_PATTERN_REG		0x48
-#define I2C_DVI_POWER_MGMT_REG			0x49
-#define I2C_DVI_LOCK_STATE_REG			0x4D
-#define I2C_DVI_SYNC_POLARITY_REG		0x56
-
-/*
- * Set VSYNC/HSYNC to active high. This is polarity of sync signals
- * from DIU->DVI. The DIU default is active igh, so DVI is set to
- * active high.
- */
-#define I2C_DVI_INPUT_DATA_FORMAT_VAL		0x98
-
-#define I2C_DVI_PLL_CHARGE_CNTL_HIGH_SPEED_VAL	0x06
-#define I2C_DVI_PLL_DIVIDER_HIGH_SPEED_VAL	0x26
-#define I2C_DVI_PLL_FILTER_HIGH_SPEED_VAL	0xA0
-#define I2C_DVI_PLL_CHARGE_CNTL_LOW_SPEED_VAL	0x08
-#define I2C_DVI_PLL_DIVIDER_LOW_SPEED_VAL	0x16
-#define I2C_DVI_PLL_FILTER_LOW_SPEED_VAL	0x60
-
-/* Clear test pattern */
-#define I2C_DVI_TEST_PATTERN_VAL		0x18
-/* Exit Power-down mode */
-#define I2C_DVI_POWER_MGMT_VAL			0xC0
-
-/* Monitor polarity is handled via DVI Sync Polarity Register */
-#define I2C_DVI_SYNC_POLARITY_VAL		0x00
 
 /*
  * DIU Area Descriptor
@@ -69,98 +36,6 @@
 #define AD_COMP_1_SHIFT		4
 #define AD_COMP_0_SHIFT		0
 
-/* Programming of HDMI Chrontel CH7301 connector */
-int diu_set_dvi_encoder(unsigned int pixclock)
-{
-	int ret;
-	u8 temp;
-	select_i2c_ch_pca9547(I2C_MUX_CH_DIU);
-
-	temp = I2C_DVI_TEST_PATTERN_VAL;
-	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_TEST_PATTERN_REG, 1,
-			&temp, 1);
-	if (ret) {
-		puts("I2C: failed to select proper dvi test pattern\n");
-		return ret;
-	}
-	temp = I2C_DVI_INPUT_DATA_FORMAT_VAL;
-	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_INPUT_DATA_FORMAT_REG,
-			1, &temp, 1);
-	if (ret) {
-		puts("I2C: failed to select dvi input data format\n");
-		return ret;
-	}
-
-	/* Set Sync polarity register */
-	temp = I2C_DVI_SYNC_POLARITY_VAL;
-	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_SYNC_POLARITY_REG, 1,
-			&temp, 1);
-	if (ret) {
-		puts("I2C: failed to select dvi syc polarity\n");
-		return ret;
-	}
-
-	/* Set PLL registers based on pixel clock rate*/
-	if (pixclock > 65000000) {
-		temp = I2C_DVI_PLL_CHARGE_CNTL_HIGH_SPEED_VAL;
-		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
-				I2C_DVI_PLL_CHARGE_CNTL_REG, 1,	&temp, 1);
-		if (ret) {
-			puts("I2C: failed to select dvi pll charge_cntl\n");
-			return ret;
-		}
-		temp = I2C_DVI_PLL_DIVIDER_HIGH_SPEED_VAL;
-		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
-				I2C_DVI_PLL_DIVIDER_REG, 1, &temp, 1);
-		if (ret) {
-			puts("I2C: failed to select dvi pll divider\n");
-			return ret;
-		}
-		temp = I2C_DVI_PLL_FILTER_HIGH_SPEED_VAL;
-		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
-				I2C_DVI_PLL_FILTER_REG, 1, &temp, 1);
-		if (ret) {
-			puts("I2C: failed to select dvi pll filter\n");
-			return ret;
-		}
-	} else {
-		temp = I2C_DVI_PLL_CHARGE_CNTL_LOW_SPEED_VAL;
-		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
-				I2C_DVI_PLL_CHARGE_CNTL_REG, 1, &temp, 1);
-		if (ret) {
-			puts("I2C: failed to select dvi pll charge_cntl\n");
-			return ret;
-		}
-		temp = I2C_DVI_PLL_DIVIDER_LOW_SPEED_VAL;
-		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
-				I2C_DVI_PLL_DIVIDER_REG, 1, &temp, 1);
-		if (ret) {
-			puts("I2C: failed to select dvi pll divider\n");
-			return ret;
-		}
-		temp = I2C_DVI_PLL_FILTER_LOW_SPEED_VAL;
-		ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR,
-				I2C_DVI_PLL_FILTER_REG, 1, &temp, 1);
-		if (ret) {
-			puts("I2C: failed to select dvi pll filter\n");
-			return ret;
-		}
-	}
-
-	temp = I2C_DVI_POWER_MGMT_VAL;
-	ret = i2c_write(CONFIG_SYS_I2C_DVI_ADDR, I2C_DVI_POWER_MGMT_REG, 1,
-			&temp, 1);
-	if (ret) {
-		puts("I2C: failed to select dvi power mgmt\n");
-		return ret;
-	}
-
-	udelay(500);
-
-	select_i2c_ch_pca9547(I2C_MUX_CH_DEFAULT);
-	return 0;
-}
-
 void diu_set_pixel_clock(unsigned int pixclock)
 {
 	unsigned long speed_ccb, temp;
@@ -172,12 +47,19 @@
 	pixval = speed_ccb / temp;
 
 	/* Program HDMI encoder */
+	/* Switch channel to DIU */
+	select_i2c_ch_pca9547(I2C_MUX_CH_DIU);
+
+	/* Set dispaly encoder */
 	ret = diu_set_dvi_encoder(temp);
 	if (ret) {
 		puts("Failed to set DVI encoder\n");
 		return;
 	}
 
+	/* Switch channel to default */
+	select_i2c_ch_pca9547(I2C_MUX_CH_DEFAULT);
+
 	/* Program pixel clock */
 	out_be32((unsigned *)CONFIG_SYS_FSL_SCFG_PIXCLK_ADDR,
 		 ((pixval << PXCK_BITS_START) & PXCK_MASK));